'''
convertImage
filter
blur
minMax
if maxValue < x:
    return None
return xAngle
'''

import cv
import math
size = None

def run(im, head):
    # conver the image
    im = convertImage(im)
    [yawHead, pitchHead] = head
    # cv.SaveImage('beforeImage.jpg', im)
    # filter the image
    im = filterImage(im)
    
    # blur the image
    blurredImage = blur(im)
    # cv.SaveImage('blurredImage.jpg', blurredImage)
    # find the max value in the image
    (maxLocation, maxValue) = findMax(blurredImage)

    # if the maxValue isn't hight enough return 'None'
    if maxValue/256.0 < 0.2:
        print 'No ball in image'
        return None
    
    (xcoord, ycoord) = maxLocation
    # calculate the left-right angle to the ball
    (xAngle, yAngle) = calcAngles((xcoord, ycoord ), (yawHead, pitchHead))
    print xAngle, yAngle
    return (xAngle, yAngle)
'''    
def calcXAngle(xcoord, yawHead):
        (width, height) = size
        xDiff = width/2 - xcoord
        # distanceToFrame = 0.5 * width/math.tan(0.5*horizontalAngle)
        # horizontalAngle = 46.4 degrees = 0.8098327729253689 radians
        # math.tan(0.5*horizontalAngle) = 0.42860054745600146
        distanceToFrame = 0.5 * width / 0.42860054745600146
        xAngle = math.atan(xDiff/distanceToFrame)
        return xAngle + yawHead	
'''	
def calcAngles(coord, head):
    (yaw, pitch) = head
    (width, height) = size
    (xcoord, ycoord) = coord
    xdiff = width/2 - xcoord
    ydiff = height/2 - ycoord
    distanceToFrame = width / 0.2143
    xAngle = math.atan(xdiff/distanceToFrame)
    yAngle = math.atan(ydiff/distanceToFrame)
    #We use the bottom camera. Therefore the angle towards the ball is different
    return (xAngle + yaw, yAngle + pitch)    

def convertImage(picture):
    global size
    size = (160, 120)
    # resize the image
    picture = picture.resize(size)
    # convert the type to OpenCV
    image = cv.CreateImageHeader(size, cv.IPL_DEPTH_8U, 3)
    cv.SetData(image, picture.tostring(), picture.size[0]*3)
    return image

def filterImage(im):
    # Size of the images
    (width, height) = size
    
    hsvFrame = cv.CreateImage(size, cv.IPL_DEPTH_8U, 3)
    filter = cv.CreateImage(size, cv.IPL_DEPTH_8U, 1)

    hueMin = 5
    hueMax = 12
    saturationMin = 80
    saturationMax = 256
    valueMin = 95
    valueMax = 256
    
    hsvMin1 = cv.Scalar(hueMin, saturationMin, valueMin, 0)
    hsvMax1 = cv.Scalar(hueMax, saturationMax, valueMax, 0)
    # Color detection using HSV
    cv.CvtColor(im, hsvFrame, cv.CV_BGR2HSV)
    cv.InRangeS(hsvFrame, hsvMin1, hsvMax1, filter)
    return filter

def blur(cv_im):
    # blur the OpenCV image
    kernelSize = 5
    cv.Smooth(cv_im, cv_im, cv.CV_BLUR, 5, 5)
    return cv_im
'''
def histogram(blurIm, histSize):
    ranges = [0, 256]
    hist = cv.CreateHist(histSize, cv.CV_HIST_ARRAY, ranges, 1)
    planes = cv.CreateImage( size, 8, 1)
    cv.CalcHist(planes, hist, 0, None)
    return hist

def countHist(hist, histSize):
    amount = 0
    amount += cv.QueryHistValue_1D(hist, histSize - 1)
    amount += cv.QueryHistValue_1D(hist, histSize - 2)
    amount += cv.QueryHistValue_1D(hist, histSize - 3)
    amount += cv.QueryHistValue_1D(hist, histSize - 4)    
    return amount
'''
    
def findMax(blurIm):
    # turn the image to grayscale   ??? is dit nodig.. is het niet al grayscale
    #blurIm_gray = cv.CreateImage(size, cv.IPL_DEPTH_8U, 1)
    #cv.CvtColor(blurIm, blurIm_gray, cv.CV_RGB2GRAY)
    minMaxLoc = cv.MinMaxLoc(blurIm)
    (minVal, maxVal, minLoc, maxLoc) = minMaxLoc
    return (maxLoc, maxVal)
